﻿namespace RLab.Visualization

open StillDesign.PhysX
open Microsoft.DirectX
open GeometryDefinitions
open PhysicConstants
open Microsoft.DirectX.Direct3D

type PhysicEngine = 
    {Core : Core
     Scene : Scene
     Mutex : obj}

type Part = 
    { Name : string
      Model : Mesh  //model mesh
      PhysicModel : V3 * V3 //physic actor: box dimensions * globalPose
      Mass : single
      CenterOfMassOffset : V3 }//model mesh offset

[<CompilationRepresentation(CompilationRepresentationFlags.ModuleSuffix)>]
module PhysicEngine = 
    let createEngine() =        
        let core = new Core(new CoreDescription(),
                    {new UserOutputStream() with 
                        member this.Print msg = printf "%A" msg
                        member this.ReportAssertionViolation ( _, _, _ ) = AssertResponse.Continue
                        member this.ReportError ( _, msg, _, _ ) = printf "%A" msg})
        core.SetParameter( PhysicsParameter.VisualizationScale, 2.0f )
        core.SetParameter( PhysicsParameter.VisualizeCollisionShapes, true )
        core.SetParameter( PhysicsParameter.VisualizeClothMesh, true )
        core.SetParameter( PhysicsParameter.VisualizeJointLocalAxes, true )
        core.SetParameter( PhysicsParameter.VisualizeJointLimits, true )
        core.SetParameter( PhysicsParameter.VisualizeFluidPosition, true )
        core.SetParameter( PhysicsParameter.VisualizeFluidEmitters, false )
        core.SetParameter( PhysicsParameter.VisualizeForceFields, true )
        core.SetParameter( PhysicsParameter.VisualizeSoftBodyMesh, true )
        core.SetParameter( PhysicsParameter.ContinuousCollisionDetection, true );
        core.SetParameter( PhysicsParameter.VisualizeContinuousCollisionDetectionTests, true );        
        let scene = 
            core.CreateScene(
                let sd = new SceneDescription()
                sd.Gravity <- gravity
                sd.GroundPlaneEnabled <- true
                sd)            
        {Core = core
         Scene = scene
         Mutex = new obj()}
    let update engine elapsedTimems = 
        lock (engine.Mutex) (fun () ->
                                engine.Scene.Simulate(elapsedTimems / 100.0f)
                                engine.Scene.FlushStream()
                                engine.Scene.FetchResults(SimulationStatus.RigidBodyFinished,true)
                                 )
    let createAnsiotropicMaterial engine sFriction dFriction=         
        let materialDesc = new MaterialDescription()
        //materialDesc.Flags <- MaterialFlag.DisableStrongFriction//  Ansiotropic
        materialDesc.Restitution <- resitution
        materialDesc.StaticFriction <- sFriction
        materialDesc.DynamicFriction <- dFriction
        materialDesc.StaticFrictionV <- sFriction
        materialDesc.DynamicFrictionV <- dFriction
        engine.Scene.CreateMaterial(materialDesc)
    let createCubeActor engine link = 
        let actorDesc = new ActorDescription(
                            let shape = new BoxShapeDescription(fst link.PhysicModel)
                            shape.Material <- createAnsiotropicMaterial engine staticFriction dynamicFriction
                            shape )
        actorDesc.Name <- link.Name        
        actorDesc.BodyDescription <- new BodyDescription(link.Mass)
        actorDesc.GlobalPose <- Matrix.Translation(snd link.PhysicModel) 
        engine.Scene.CreateActor(actorDesc)
    let createCubeActor2 engine link sFriction dFriction = 
        let actorDesc = new ActorDescription(
                            let shape = new BoxShapeDescription(fst link.PhysicModel)
                            shape.Material <- createAnsiotropicMaterial engine sFriction dFriction
                            shape )
        actorDesc.Name <- link.Name        
        actorDesc.BodyDescription <- new BodyDescription(link.Mass)
        actorDesc.GlobalPose <- Matrix.Translation(snd link.PhysicModel) 
        engine.Scene.CreateActor(actorDesc)
    let createJointDescription (ctor : unit -> #JointDescription) a1 a2 vAnchor vAxis =
        let jDesc = ctor()
        jDesc.Actor1 <- a1
        jDesc.Actor2 <- a2               
        jDesc.SetGlobalAnchor(vAnchor)
        jDesc.SetGlobalAxis(vAxis)
        jDesc.JointFlags <- JointFlag.CollisionEnabled ||| JointFlag.Visualization        
        jDesc
    let createJoint (ctor : unit -> #JointDescription) engine a1 a2 vAnchor vAxis =
        createJointDescription ctor a1 a2 vAnchor vAxis 
            |> engine.Scene.CreateJoint
    let lockAllDegreesOfFreedom (jDesc : D6JointDescription) = 
        jDesc.Swing1Motion <- D6JointMotion.Locked
        jDesc.Swing2Motion <- D6JointMotion.Locked
        jDesc.TwistMotion  <- D6JointMotion.Locked
        jDesc.XMotion      <- D6JointMotion.Locked
        jDesc.YMotion      <- D6JointMotion.Locked
        jDesc.ZMotion      <- D6JointMotion.Locked
        jDesc
    let createFixedJoint engine a1 a2 = 
        let desc = new FixedJointDescription()
        desc.Actor1 <- a1
        desc.Actor2 <- a2
        engine.Scene.CreateJoint(desc)
    let createRevoluteJoint engine a1 a2 (joint : ChainJoint) = 
        let j = createJoint ( fun () -> 
                    let desc = new RevoluteJointDescription() 
                    desc.Flags <-  RevoluteJointFlag.LimitEnabled ||| RevoluteJointFlag.MotorEnabled
                    desc.Motor <- 
                        let moD = new MotorDescription(-0.5f,100.0f,false)                        
                        moD
                    desc.Spring <-
                        let spring = new SpringDescription(
                                        jointSpringParameters.X,
                                        jointSpringParameters.Y,
                                        jointSpringParameters.Z)
                        spring
                    desc.Name <- joint.Name
                    desc.AccelerationSpring <- JointAccelerationSpringType.Acceleration
                    desc ) engine a1 a2 joint.Anchor joint.Axis
        j     
    let createPrismaticJoint engine a1 a2 (joint : ChainJoint) =    
        let createPrismaticDriveInfo jType = 
            let drive, motion = 
                match jType with
                | VelocityDrive -> 
//todo: send it to constants
                    Some(new JointDriveDescription(0.1f,D6JointDriveType.DriveVelocity,10000000.0f,1.0f)), D6JointMotion.Limited
                | ForceDrive    -> 
//todo: send it to constants
                    Some(new JointDriveDescription(0.01f,D6JointDriveType.DrivePosition,0.1f,10.09f)),D6JointMotion.Limited
                | NoDrive -> 
                    None,D6JointMotion.Free
//todo: send it to constants. Can be universal for velocity and force?
            drive,motion,new JointLimitSoftDescription(0.f, 0.0f, 900.0f, 40.0f)    
        let jDesc = 
            createJointDescription (fun () -> new D6JointDescription()) a1 a2 joint.Anchor joint.Axis
            |> lockAllDegreesOfFreedom
        jDesc.Name <- joint.Name        
        jDesc.SolverExtrapolationFactor <- defaultSolverExtrapolationFactor
        let drive, motion, limit = 
            createPrismaticDriveInfo 
                (match joint.Type with
                 | Prismatic(drivetype) -> drivetype
                 | _ -> failwith "Invalid joint")
        match drive with
        | Some(d) -> jDesc.XDrive <- d
        | None -> ()
        jDesc.XMotion <- motion
        jDesc.LinearLimit <- limit                
        (jDesc |> engine.Scene.CreateJoint)        
    let createChainJoint engine (joint : ChainJoint) a1 a2 = 
        let f = match joint.Type with
                | Revolute  -> createRevoluteJoint
                | Prismatic(_) -> createPrismaticJoint
        f engine a1 a2 joint
    let createLimits low high = 
        let mutable l = new JointLimitPairDescription() 
        l.Low <- new JointLimitDescription(low, 0.0f, 0.0f)
        l.High <- new JointLimitDescription(high, 0.0f, 0.0f)
        l
    let moveRevoluteTo engine angle velocity (joint : RevoluteJoint) =           
        let s = lock (engine.Mutex) (fun() ->
                                let low,high,sign = 
                                    if angle >= joint.Angle then 
                                        joint.Angle-0.2f,angle+0.00009f,1 
                                    else 
                                        angle-0.00009f,joint.Angle+0.2f, -1
                                joint.Limits <- createLimits low high         
                                joint.Motor <- new MotorDescription(velocity * (single sign),driverMaxForce,true)
                                sign)
        if s > 0 then
            while System.Math.Round( (float)joint.Angle,4) < System.Math.Round( (float)angle,4) do
                        System.Diagnostics.Trace.WriteLine(joint.Name + " " + joint.Angle.ToString())
        else
            while System.Math.Round( (float)joint.Angle,4) > System.Math.Round( (float)angle,4) do
                        System.Diagnostics.Trace.WriteLine(joint.Name + " " + joint.Angle.ToString())
        System.Diagnostics.Trace.WriteLine(joint.Name + " moved")
    
    let linearDriveJointPosition (joint : D6Joint) =                         
        let pos = (joint.GlobalAnchor - new Vector3( joint.Actor1.GlobalPose.M41,joint.Actor1.GlobalPose.M42, joint.Actor1.GlobalPose.M43))
        let axis = joint.GlobalAxis
        let result = pos.Length() * (cosinus axis pos)        
        result
    let movePrismaticTo engine position velocity joint =
        let sign,jointDesc =
            lock engine.Mutex (fun () ->
                                let curr = linearDriveJointPosition joint
                                let abs x = if x < 0.f then -1.f*x else x
                                let sign = if position >= curr then 1.f else -1.f
                                let jointDesc = joint.SaveToDescription()
                                //todo: send it to constants. Can be universal for velocity and force?
                                jointDesc.LinearLimit <- new JointLimitSoftDescription(300000.f, 0.0f, 900.0f, 40.0f)         
                                joint.LoadFromDescription(jointDesc)                        
                                joint.SetDriveLinearVelocity(V3(velocity*sign,0.f, 0.f))
                                sign,jointDesc)
        if sign > 0.f then
            while (linearDriveJointPosition joint) < position do
                System.Diagnostics.Trace.WriteLine(joint.Name+" "+(linearDriveJointPosition joint).ToString())
        else
            while (linearDriveJointPosition joint) > position do
                System.Diagnostics.Trace.WriteLine(joint.Name+" "+(linearDriveJointPosition joint).ToString())
        System.Diagnostics.Trace.WriteLine(joint.Name+" "+(linearDriveJointPosition joint).ToString())
        System.Diagnostics.Trace.WriteLine(joint.Name + " moved")
        lock engine.Mutex (fun () ->
                                jointDesc.LinearLimit <- new JointLimitSoftDescription((abs position)*2.f, 0.0f, 900.0f, 40.0f)                                
                                joint.LoadFromDescription(jointDesc)                                
                                joint.SetDriveLinearVelocity(V3(0.001f*sign,0.f, 0.f)))
    let createPrismaticJoint_TOOL engine a1 a2 (joint : ChainJoint) =    
        let createPrismaticDriveInfo jType = 
            let drive, motion = Some(new JointDriveDescription(2.1f,D6JointDriveType.DrivePosition,0.1f,1.09f)),D6JointMotion.Free
            drive,motion
        let jDesc = 
            createJointDescription (fun () -> new D6JointDescription()) a1 a2 joint.Anchor joint.Axis
            |> lockAllDegreesOfFreedom
        jDesc.Name <- joint.Name        
        jDesc.SolverExtrapolationFactor <- 0.57f//defaultSolverExtrapolationFactor
        let drive, motion = 
            createPrismaticDriveInfo 
                (match joint.Type with
                 | Prismatic(drivetype) -> drivetype
                 | _ -> failwith "Invalid joint")
        match drive with
        | Some(d) -> jDesc.XDrive <- d
        | None -> ()
        jDesc.XMotion <- motion
        //jDesc.LinearLimit <- limit                
        (jDesc |> engine.Scene.CreateJoint)
    let movePrismaticTo_TOOL engine position velocity (joint : D6Joint) =
        joint.SetDrivePosition(V3(position,0.f,0.f))        

